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ABSTRACT 


The  PLANS  (Primary  Land  Arctic  Navigation  System),  developed  at  DREO,  optimally 
integrates  a  directional  gyro/gyrocompass,  an  odometer,  a  3-axis  strapdown  magnetometer,  a  GPS 
receiver,  a  Transit  receiver,  a  baroaltimeter  and  a  digital  terrain  elevation  map,  for  the  purpose  of 
navigating  a  land  vehicle  in  the  Canadian  Arctic  under  potentially  adverse  conditions.  This  report 
derives  the  exact  form  of  the  discrete  driving  noise  covariance  matrix  which  is  needed  to  propagate 
the  covariance  matrix  in  the  Kalman  fdter  used  by  PLANS.  It  is  shown  that  the  exact  does  not  have 
a  Cholesky  UDU^  decomposition.  However,  a  good  approximation  is  shown  to  have  the  necessary 
decomposition  for  use  in  the  Biermann-Agee-Tumer  formulation  of  the  Kalman  filter.  This 
approximate  decomposition  is  then  found.  A  general  result  on  the  preservation  of  block  diagonal  form 
under  UDU^  decomposition  is  also  proven. 


R^:SUMfi 


Le  syst^me  de  navigation  terrestre  PLANS,  congu  au  CRDO,  intfegre  de  fagon  optimale  un 
gyroscope  a  deux  modes  (gyrocompas  et  directionnel),  un  odomStre,  une  sonde  magn6tique  a  trois 
axes,  un  rdcepteur  GPS,  un  rdcepteur  TRANSIT,  un  altim^tre  baromdtrique  ainsi  qu’un  carte 
d’dldvation  numdrique.  PLANS  a  dtd  congu  pour  opdrer  dans  I’arctique  canadien  ^  bord  de 
vdhicules  terrestres.  Ce  rapport  prdsente  la  formulation  exacte  de  la  matrice  de  covariance  Q|( 
ndcessaire  pour  la  propagation  de  la  matrice  de  covariance  du  filtre  Kalman  utihsd  par  PLANS.  II  est 
ddmontrd  que  Qj^  ne  peut  etre  ddcomposd  seion  la  mdthode  Cholesky  UDU^.  II  est  toutefois 
ddmontrd  qu’on  peut  obtenir  d’une  bonne  approximation  la  ddcomposition  ndcessaire  pour  utiliser 
la  formulation  Biermann-Agee-Tumer  du  filtre  Kalman.  Cette  ddcomposition  approximative  est 
ddmontrde.  II  est  aussi  ddmontrd  que  la  ddcomposition  UDU^  prdserve  la  forme  diagonale. 
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EXECUTIVE  SUMMARY 


PLANS  (Primary  Land  Arctic  Navigation  System)  is  a  multi-sensor  integrated  navigation 
system  developed  at  DREO.  PLANS  employs  an  8  state  Kalman  filter  to  optimally  integrate  the  sensor 
data  from  a  Transit  receiver,  a  GPS  receiver,  a  gyrocompass/directional  gyro,  an  odometer,  a 
magnetometer  and  a  baroaltimeter.  In  the  process  of  deriving  and  implementing  the  Kalman  filter 
equations,  one  of  the  many  matrices  that  must  be  found  is  the  discrete  process  noise  covariance  matrix 
Qjj  (also  known  as  the  driving  noise  covariance). 

Initially,  as  is  common  practice,  an  approximation  was  used  to  evaluate  this  Q|(.  During  a 
detailed  analysis  of  simulation  results,  the  behaviour  of  the  PLANS  position  error  covariance  matrix,  P, 
under  propagation  (i.e.  without  position  measurements  from  Transit  or  GPS)  came  under  suspicion. 
This  behaviour  is  governed  solely  by  the  state  transition  matrix 4>(tAt)  (at  time  t  over  an  interval  At) 
and  the  driving  noise  covariance,  Qj^.  The  discrete  driving  noise  covariance  matrix,  Qjj,  over  the 
interval  A  t,  is  itself  defined  by  the  continuous  driving  noise  power  spectral  density  matrix,  Q,  and  the 
state  transition  matrix,  4>(t,At).  Therefore  these  matrices  came  under  special  scrutiny.  Since  4>(tAt) 
was  already  exact  this  then  led  to  the  desire  for  a  more  exact  Qj^.  The  purpose  of  this  report  is 
therefore  to  derive  the  "exact"  form  of  Qjj  (and  find  its  Cholesky  decomposition  for  use  in  PLANS). 

Since  PLANS  employs  a  "square  root"  formulation  of  the  Kahnan  filter  equations  for  improved 
numerical  stability,  it  is  therefore  necessary  to  find  the  Cholesky  UDU'^  decomposition  of  Qj^  (where 
U  is  an  upper  triangular  matrix  and  D  is  diagonal).  For  the  original  approximation,  was  diagonal  so 
that  its  decomposition  was  trivial.  With  the  more  exact  Qjj  this  is  no  longer  the  case.  Furthermore  the 
exact  Q|(  is  not  constant,  so  that  the  use  of  a  numerical  routine  to  find  its  decomposition  would  require 
considerable  computation,  making  an  explicit  decomposition  highly  desireable. 

It  is  proven  in  this  report  that  in  general  UDU^  decomposition  preserves  block  diagonal  form, 
and  therefore  that  the  process  of  finding  em  exact  decomposition  of  a  large  block  diagonal  matrix  can 
be  reduced  to  the  much  simpler  problem  of  decomposing  the  smaller  blocks.  This  is  then  applied  to  the 
Qj(  for  PLANS,  so  that  instead  of  having  to  decompose  an  8x8  matrix,  it  is  only  necessary  to  decompose 
three  1x1  matrices  (which  is  trivial),  one  2x2  matrix  and  one  3x3  matrix.  The  2x2  matrix  is  easily 
decomposed  exactly.  The  3x3  matrix  however  causes  some  difficulty.  Its  UDU^  decomposition  is  easily 
enough  found,  however  it  is  not  a  Cholesky  decomposition  because  its  D  component  has  negative 
diagonal  elements.  This  is  not  particularly  surprising  since  the  3x3  matrix  is  not  positive  definite  and 
therefore  the  existence  of  its  Cholesky  decomposition  is  not  guaranteed. 

It  is  thus  shown  that  a  Cholesky  decomposition  of  the  exact  Q|(  is  not  possible,  and  an 
approximation  is  still  required.  An  appronmation  is  found  which  is  exact  for  all  but  a  few  of  the  small 
off-diagonal  terms  of  Qjj. 

As  it  turned  out,  the  "i-uspicious"  behaviour  of  the  error  state  covariance  matrix  P  was  not  due 


to  the  inexactness  of  Qj^,  but  in  fact  couJd  be  explained  by  closer  examination  of  the  effect  of  4> . 

Although  in  hindsight  this  behaviour  seems  obviously  correct,  the  suspicion  arose  because  of  the 

intuitive  expectation  that  the  position  uncertainty  (represented  by  bottom  right  2x2  block  of  P,  since  the  ^ 

position  error  states  are  the  last  two  elements  of  the  state  vector)  should  increase,  or  at  least  not 

decrease,  in  the  absence  of  position  measurements.  Although  it  is  true  that  the  covariances  of  most 

elements  of  the  state  vector  behave  in  this  way  (since  they  are  independent  Markov  processes),  it  is  not  '* 

generally  true  for  the  position  covariance.  This  is  because  of  a  geometric  effect  which  can  lead  to  a 

canceUation  of  errors  in  some  situations,  such  that  the  position  covariance  locaUy  decreases.  In  the  case 

of  PLANS  this  could  be  due  to  the  effect  of  the  speed  and  heading  errors  while  returning  to  the 

starting  point  partiaUy  cancelling  the  errors  accumulated  during  the  outbound  portion  of  the  trip  (a 

perfectly  constant  heading  and  speed  error  would  perfectly  cancel  if  movement  were  on  a  plane).  Since 

this  geometric  effect  is  correctly  modelled  in  the  PLANS  Kalman  filter,  the  covariance  behaves 

accordingly. 

Although  the  "problem"  that  this  effort  was  intended  to  solve  turned  out  not  to  be  a  problem,  a 
more  exact  form  of  the  driving  noise  covariance  is  of  course  desireable  in  any  case,  and  the  general 
result  on  preservation  of  block  diagonal  form  under  decomposition  is  also  quite  useful. 
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1.  INTRODUCTION 


Reference  [1]  describes  the  multi-sensor  integrated  navigation  system  called  PLANS  (Primary 
Land  Arctic  Navigation  System),  along  with  the  8  state  Kalman  filter  used  for  integration  of  the  sensor 
data.  In  the  process  of  deriving  and  implementing  the  Kalman  filter  equations,  one  of  the  many 
matrices  that  must  be  found  is  the  discrete  process  noise  covariance  matrix,  (also  known  as  the 
driving  noise  covariance).  In  reference  [2]  an  approximation  was  used  to  evaluate  this  Qjj.  During  a 
detailed  analysis  of  simulation  results,  the  behaviour  of  the  PLANS  position  error  covariance  matrix,  P, 
under  propagation  (i.e.  without  position  measurements  from  Transit  or  GPS)  came  under  suspicion. 
This  behaviour  is  governed  solely  by  the  state  transition  matrix,  4>(tAt),  and  the  driving  noise 
covariance,  Qj^,  as  follows; 


**1  +At  ~ 


^(tAOPt^  (tAO  +  Qk 


(1) 


where  the  discrete  driving  noise  covariance  matrix,  Qjj,  over  the  interval  At  is  defined  by  the 
continuous  driving  noise  power  spectral  density  matrbc,  Q,  and  the  state  transition  matrix,  (t A  t),  as 
follows  (see  for  example  reference  [SJ); 


At 

Ok  =  |4>(t,T)Q$T(t^)  dT  (2) 

0 


Therefore  these  matrices  came  under  special  scrutiny,  which  then  led  to  the  desire  for  a  more 
exact  Ok,  since  ^*(t,T)  was  already  exact.  The  purpose  of  this  report  is  therefore  to  derive  the  "exact" 
form  of  Qk  and  find  its  Cholesky  decomposition  for  use  in  PLANS. 

Since  PLANS  employs  a  "square  root"  formulation  of  the  Kalman  filter  equations  for  improved 
numerical  stability  (see  reference  [3|),  it  is  therefore  necessary  to  find  the  Cholesky  UDU^ 
decomposition  of  Qk  (where  U  is  an  upper  triangular  matrix  and  D  is  diagonal).  For  the  original 
approximation,  Qk  was  diagonal  so  that  its  decomposition  was  trivial.  With  the  more  exact  Qk  this  is  no 
longer  the  case.  Furthermore  the  exact  Qk  is  not  constant,  so  that  use  of  a  numerical  routine  to  find  its 
decomposition  would  require  considerable  computation,  making  an  explicit  decomposition  highly 
dcsireable. 
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In  fact  it  turns  out  that  a  Cholesky  decomposition  of  the  exact  is  not  possible,  as  will  be 
shown  below,  and  an  approximation  is  still  required.  This  approximation  however  only  involves  some  of 
the  small  off-diagonal  terms,  and  is  exact  for  most  terms  of  Q|(. 

As  it  turned  out,  the  "suspicious"  behaviour  of  the  error  state  covariance  matrbt  P  was  not  due 
to  the  inexactness  of  Qjj,  but  in  fact  could  be  explained  by  closer  examination  of  the  effect  of  4' . 
Although  in  hindsight  this  behaviour  seems  obviously  correct,  the  suspicion  arose  because  of  the 
intuitive  expectation  that  the  position  uncertainty  (represented  by  bottom  right  2x2  block  of  P,  since  the 
position  error  states  are  the  last  two  elements  of  the  state  vector)  should  increase,  or  at  least  not 
decrease,  in  the  absence  of  position  measurements.  Although  it  is  true  that  the  covariances  of  most 
elements  of  the  state  vector  behave  in  this  way  (since  they  are  independent  Markov  processes),  it  is  not 
generally  true  for  the  position  covariance.  This  is  because  of  a  geometric  effect  which  can  lead  to  a 
cancellation  of  errors  in  some  situations,  such  that  the  position  covariance  locally  decreases.  In  the  case 
of  PLANS  this  could  be  due  to  the  effect  of  the  speed  and  heading  errors  while  returning  to  the 
starting  point  partially  cancelling  the  errors  accumulated  during  the  outbound  portion  of  the  trip  (a 
perfectly  constant  heading  and  speed  error  would  perfectly  cancel  if  movement  were  on  a  plane).  Since 
this  geometric  effect  is  correctly  modelled  in  the  PLANS  Kalman  filter,  the  covariance  behaves 
accordingly. 

Although  the  "problem"  that  this  effort  was  intended  to  solve  tiumed  out  not  to  be  a  problem,  a 
more  exact  form  of  the  driving  noise  covariance  is  of  course  desireable  in  any  case,  and  the  general 
result  of  Appendbc  A  is  also  quite  useful. 


2.  THE  CONTINUOUS  Q  MATRIX  FOR  PLANS 


Since  the  discrete  Q|^  matrix  is  found  by  integrating  the  continuous  power  spectral  density 
matrix,  Q,  folded  with  the  propagation  matrix  ^(t,T),  as  in  (2),  we  must  first  examine  these  two 
matrices. 


As  was  shown  in  reference  [2]  the  PLANS  error  state  vector  contains  5  independent  first  order 
Markov  processes  and  three  states  which  are  derived  from  the  integrals  of  these  Markov  processes. 
Thus  the  continuous  power  spectral  density  matrix  Q  has  diagonal  elements  for  each  of  these  Mjukov 
processes,  as  follows: 


O 


f 


V 


qj  0  0  O' 
0  q2  0  0 
0  0  q3  0 
0  0  0  q4 
0  0  0  0 
0  0  0  0 
0  0  0  0 
0  0  0  0 


0  0  0  0  ' 
0  0  0  0 
0  0  0  0 
0  0  0  0 
0  0  0  0 
0  q6  0  0 
0  0  0  0 
0  0  0  0 


(3) 


where  the  q;  are  the  constant  values  of  the  PSD’s  (power  spectral  densities)  of  the  white 
driving  noise  for  each  of  the  individual  Markov  states.  These  can  be  expressed  in  terms  of  the  standard 
Markov  process  error  model  parameters  (correlation  time  Ti  and  steady  state  covariance  pi )  as  foUows 
(see  any  standard  text,  such  as  [5]): 


qi  =  2pi/Ti 


(4) 


The  PLANS  error  state  transition  matrix,  ^>(t,r),  is  also  derived  in  reference  [2],  where  it  is 
shown  to  be  (at  time  t,  over  the  interval  r) : 
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0  0  0 

0  0  0 

0  0  0 
0  0  0 
0  0  0  T4(l-e“^/T4) 


0 

0 

0 


0  0 
0  0 
0  0 


0 

0 

0 


0  0  0  0 

0  0  0  0 

0  0  0  0 

0  0  0  0 

1  0  0  0 
e-r/T6  q  0  0 

-TSsinS  TScobB  1  0 
TScxseS  TSeisS  0  1 


(5) 


* 


I 


where  the  Ti  are  Markov  process  correlation  times  (constants),  S(t)  is  the  vehicle  speed,  6  (t)  is 
the  vehicle  heading  and  T  is  the  propagation  period. 
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3.  THE  DISCRETE  Qk  MATRIX  FOR  PLANS 


The  discrete  driving  noise  covariance  matrix,  Qjj,  over  the  interval  At,  is  defmed  by  the 
continuous  driving  noise  power  spectral  density  matrix,  Q,  and  the  state  transition  matrix,  ^(t,T),  as 
shown  in  equation  (i)' 


At 

Qk  =  |$(t,T)Q^>T(t,T)  dr  (6) 

0 

where  from  equations  (3)  and  (5)  we  can  see  that  the  integrand  is: 


$0|)T  = 


r 


0  0  0 

0  0”T/T2  q  q 

0  0  0 
0  0  0 
0  0  0  T4(l-e'^/r‘^ 

0  0  0  0 

0  0  0  0 

0  0  0  0 


0  0  0  0 

0  0  0  0 

0  0  0  0 

0  0  0  0 

1  0  0  0 
e-r/T6  0  0  0 

-TSsirlP  TScxJsfl  1  0 
7SCOS0  TSsinP  0  1 


A 


J 


<31 


92 


54 


(7) 


Now  from  this  we  can  a'-^eady  see  that  Qk  will  have  the  same  block  diagonal  form  as4>,  namely 
three  1 -blocks  and  a  5-block.  As  shown  in  Appendix  A,  this  block  diagonal  form  is  also  preserved  under 
UDLJT^  decomposition.  Therefore  the  top  three  1-blocks  have  trivial  decompositions,  since  they  are 
already  diagonal.  Thus  for  i  =  1,2,3  we  have: 


5 


At 


Qk(j,i) 


dr 


=  ^qi(l-e-2^t/Ti) 


Now  as  seen  in  equation  (4)  above,  for  the  steady  state  Markov  process  xj  the  magnitude  of 
the  PSD  of  the  white  driving  noise  is  qj,  which  is  related  to  the  steady  slate  covariance  pi  and  the 
correlation  time  Ti  according  to: 

qi  =  2pi/Ti 

Substituting  this  into  equation  (10)  gives,  for  i  =  1,2,3; 


Qk(i,i)  =  Pi(l  -  e-2At/Ti) 


We  will  now  restrict  our  attention  to  the  remaining  5-block.  Since  this  is  non-diagonal,  it 
requires  a  non-trivial  UDU^  decomposition.  Henceforth  for  simplicity  Q,  Qj^  and  $  shall  refer  to  the 
corresponding  5-blocks  rather  than  the  full  matrices.  Thus,  from  (7)  we  have: 


4>04>T  = 


0  0  0  0 

T4  (l-e“’’/T4)  1  0  00 

0  0  0”7’/'^6  Q  Q 

0  -TSsinfi  TScosO  1  0 

0  TScosd  TSaind  0  1 


q4  0  0  0  0 
0  0  0  0  0 
0  0  qg  0  0 

0  0  0  0  0 
0  0  0  0  0 
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q4e“^/T4  0  0  0  0  ' 

^  e“^/T4  T4(l-e'^^'^)  0  0  0  1 

qjT4(l^-r/T4)  q  0  0  0 

0  1  0  -TSsin^  iScoaB  I 

0  0  q6e“^/T6  0  0 

0  0  tScosB  tSbisS  1 

0  0  7q5Sooa0  0  0 

0  0  7q5Ssin0  0  0 

C)  o 

o  o 

o  o 

O 

!-•  o 

(13) 


^-2r/T4  0  0  0 

cyjT4{l^-"/T4^-r/T4  0  0  0 

0  0  rqgSco^e^/'^^  Tq5Ssirfe'’‘/'^^ 

0  0  Tq^Sco^e^/'^®  q^S^cos^r^ 

0  0  Tq^Ssii^e'’'/^^  q5S2sirfco^ 


(14) 


It  is  now  clear  that  this  Q^  also  has  a  block  diagonal  form,  with  a  2-block  and  a  3-block. 
Assuming  that  the  time  dependent  terms  (speed  S  and  heading  0)  are  constant  over  the  integration 
interval,  the  above  matrix  can  be  expUcitly  integrated  (as  in  equation  (6)).  For  convenience  we  will 
label  the  individual  terms  as  follows: 


f  ‘111  ^12  0  ®  ^  -X 

^21  ^22  0  ®  ® 

C  0  q33  q34  q35 

0  0  q43  q44  q45 

0  0  q53  q54  qss 


(15) 


where  by  symmetry  qjj  =  qj;.  Then  qn  can  be  found  as  in  equations  (8)  to  (11)  above: 


<211 


At 

=  |q4e-2^4d, 

0 


(16) 
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-  fq,,!  - 


(17) 


=  p4(l  -  e-2At/T4) 


(18) 


Similarly  for  the  other  components; 


At 


‘312 


=  |T4q4(l  - 


T42  ,  -T/T4i2 

=  —  q4(l  -  e 


T42 


q4(l  - 


=  T4  p4  ( 1  - 


(19) 


At 


q33  =  1^6® 


p6(l  -  (as  in  q^i) 


(20) 
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^14  3 


Similarly 


<353 


<354 


<344 


At 

=  I  qgScosSTe'^'^'^dr 
0 


-t/T6 

=  q^ScoS^ _ (-T/T6  - 1) 

1/T6)2 


=  -T6qgScos0(e  +  T6)  -  T6] 

=  2T6p6Scos0(l  -  +  At/T6)] 


(21) 


At 

=  I  qgSaind  dr 

0 


=  2T6p6Ssin0(l  -  +  At/T6)]  (22) 


At 

qgS^8in0cos0  dr 

0 


At 

0 


=  q^2a,  nflrnu^ 


(23) 


At 

=  JqgS^cos^^T^dr 
0 
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=  ggS^cos^^lf-l 

3  1  0 

=  qgS^cos^^ 

(24) 

1 

Similarly 

<355 

At 

=  1  qgS^sin^flr^dr 

0 

=  q6s2ain20^|^‘ 

-  qsS^sin^e^ 

(25) 

<322 

At 

=  jT42q4(l  -  e“’’^'^^)2dr 

0 

- 

At 

=  T42q4  1  (1  -  2e-’'/T4  + 

0 

=  T42q4(T+  2T4e“^^'^4  _  ^^-2T/T4)  |  ^ 

=  T42q4(At  +  2T4e“<^^^'^^  -  ^e-2Atrr4  _  2X4  + 

=  T43q4(At/T4+  -  ^-2At/r4  _  Ij 

’ 

=  TaS  / 2At  +  4p-At/T4  _  g-2At/r4  _  3)p4 

T4 

(26) 
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Now  by  substituting  equations  (18)  through  (26)  into  equation  (15)  we  can  write  the  discrete 
5X  5  Qjj  matrix  as  follows: 


Qk  = 


P4(i-e^t/r‘^ 

T‘^4(l-e'^‘^V 

0 

0 

0 

T4p4(l-€'^‘-^V 

T42p4^  +4e'^‘/^V^‘^‘*-3] 
T4 

1  0 

0 

0 

0 

0 

j^l^-2At/Ib)  2T6Sco^Ap5 

2T6Ssii^Apg 

0 

0 

2T6Sco^Ap6  2At3s2cos^p^ 
3T6 

2  At^ 

3T6 

0 

0 

7r68<;it^A  p^  2At^  ,q2^ii^rrv^pg 
3T6 

2At^s2^in^pg 

3T6 

(27) 

where: 

A  =  1  -  e-^t/T6(i  .  Il,  (28) 


As  indicated  in  reference  [1],  the  correlation  times  (T4  and  T6)  and  steady  state  covariances 
(p4  and  p6)  for  the  Markov  processes  representing  the  error  in  the  gyro  drift  rate  and  the  odometer 
scale  factor  are  assumed  to  be  constants.  Therefore  it  is  easy  to  see  how  this  process  driving  noise 
covEU'iance  matrix  Qj^  behaves  numerically  for  different  discretization  intervals  At.  Using  the  values 
given  in  [2],  we  have  for  At  =  60  seconds: 


Ok  = 


0.03p4  P4 
P4  '^P4 


0 

0 


0 

0 


0 

0 


0  0  0.03p^  237p^Sco^  237pgSsiitf 

0  0  237p5Sco^  40p^S^cos^  40pgS^sirfco^ 

0  0  237p(^Ssiif  40p^S^sirfcosS  40p5S^sin% 


(29) 
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and  for  At  =  1  second: 


Qk 


0.0(X)5p4  0.(XX)3p4  0  0  0 

0.0003p4  0.001p4  0  0  0 

0  0  0.0005p6  0.0003  pgSco^  0.0003p6Ssirf 

0  0  0.0003  pgSco^  0.0002p5S^cos^  0.0002  p^S^sirfcos^ 

0  0  0.0003  pgSsii^  0.0002p^S^sirfco^  0.0002p5S^sin% 

I.  > 

This  now  allows  us  to  see  the  relative  significance  (or  insignificance)  of  the  off-diagonal  terms. 


(30) 
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4.  UDUT  decomposition  of  Qk 


To  decompose  the  Qjj  matrix  for  use  in  one  of  the  numerically  superior  "square  root" 
formulations  of  the  Kalman  filter  (see  for  example  reference  [3]),  we  follow  Bierman  in  using  the 
Cholesky  factorization  method.  For  this  we  must  find  matrices  U  and  D  such  that  U  is  upper  triangular 
with  I’s  on  the  diagonal,  D  is  diagonal  and  positive  semi-definite,  and 


QIj  =  uduT 


(31) 


Since  Oj^  is  of  block  diagonal  form,  it  can  be  easily  shown  that  its  square  root  has  the  same 
block  diagonal  form,  and  by  simple  extension  so  must  its  U  factor  (as  is  proven  in  Appendix  A  below). 
Therefore  the  factorization  can  be  greatly  simplified  by  performing  it  separately  on  the  diagonal  blocks 

ofQk- 


4.1.  EXACT  DECOMPOSITION  OF  THE  2X2  BLOCK 


A  general  factorization  for  a  2x2  block  can  be  found  as  follows.  We  equate  the  general  matrix 
to  the  UDU^  product,  where  U  and  D  are  of  the  required  form: 


a  b 
b  c 


■  1  d 
0  1 


(32) 


e  fd 

10 

Of 

.  d  1  , 

(e+fd^ ) 

fd 

fd 

f 

(33) 


We  then  solve  for  the  unknown  elements  of  U  and  D  (e,  f  and  d)  as  functions  of  the  elements 
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of  the  general  matrix  (a,  b  and  c).  Therefore,  by  inspection,  the  exact  solution  is; 


f 

c 

d 

b/c 

e  = 

a  -  b^/c 

(34) 


This  can  now  be  used  to  find  the  decomposition  of  the  2x2  block  of  Qj^,  as  given  in  (27).  Thus 
we  t£ike  a,  b  and  c  are  from  equation  (27)  and  substitute  into  (34).  The  resulting  expressions  can  be 
simplified  as  follows. 


f  =  c  =  Qj((2,2) 


2  At 

T4 


^t/T4 

+  4e 


-2At/T4  1 

e  -  3 


(35) 


.^.2/ 2At  r.  At  ^  (-At/T4)2  (-At/T4)3, 

=  p4T42'^-^  +  4ll-Y4'^  2  31  J 


-  [1  - 


2At  (-2At/T4)^  (-2At/T4)3 


T4 


3! 


]  -  3  } 


(where  we  have  used  the  fu-st  4  terms  of  the  Maclaurin  series  expansion  for  e*) 


'  p4T42{^  +4.4t  ♦ 
=  p4T4^f[w) 


2p4At3  (36) 

3T4 


This  will  be  a  good  approximation  provided  that  the  discretization  interval  At  is  significantly 
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less  than  the  correlation  time  T4  (which  it  will  be  in  PLANS). 

Now  the  next  term  can  also  be  simplified  by  similarly  using  the  Maclaurin  expansion  and 
ignoring  the  higher  order  terms  in  At/T4 ; 

d  =  b/c 

-At/T4 

=  T4p4(  1  -  e  )2/c  (37) 


T4p4(  1  -  1  +  At/T4)^ 
2p4At3 
3T4 


T4( 

(At/T4)2 

2At3 

L  3T4 

=  _3_  (38) 

2  At 

Finally  the  third  term  can  also  be  simplified: 


e 


a  -  b^/c 


=  p4(l-e-2^^/^'*)  - 


T42p4^1-e’^‘/^V 

p4T42  2At  +  4e-Al/T4.e-2:\t/T4  .3 

T4 

V 


(39) 


2p4At  3T4^p4At 


T4 


2T4^ 
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(2  -  3/2) 


p4At 

T4 


p4At 

2T4 


(40) 


In  this  case  the  exact  solution  of  (34)  is  given  by  (35),  (37)  and  (39),  with  a  good  approximation 
given  by  (36),  (38)  and  (40): 


f 

2  2p4At^ 

3T4 

d 

=  3 

2  At 

^  p4At 

c 

“  2T4 

(41) 


4.2.  EXACT  DECOMPOSITION  OF  THE  3X3  BLOCK 


The  decomposition  of  the  3x3  block  can  be  found  in  a  similar  way: 


a  b  c 

'  1  h  i  ■ 

'  k  0  0 

10  0' 

b  d  e 

= 

0  1  j 

0  L  0 

h  1  0 

c  e  g 

.001 

0  0m 

.  i  j  1  . 

k  hL  im 

10  0 

0  L  jm 

h  1  0 

0  0  m 

.  i  j  1  . 

(k+h^L+i^m)  (hL+ijm)  im 

(hL+ijm)  (L+j^m)  jm 
im  jm  m  ^ 


(42) 


(43) 
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Therefore  we  have: 
m  =  g 


(44) 


jm  =  e 

=>  j  =  e/m 

=  e/g 

im  =  c 

=*  i  =  c/m 

=  c/g 


(45) 


(46) 


L+ j^m 


d 

L  =  d  -  e^/m 
=  d  -  e^/g 


(47) 


hL+ijm  =  b 


(b  -  ijm)/L 


b  -  ce/m 
d  -  e^g 


b  -  ce/q 
d  -  e^g 


(48) 


k 


-  h2L  -  i2 


m 


-  <V  -  c2/, 

d  -  e^g 


(4Q) 


Now  when  the  actual  values  for  a,  b,  c,  d,  e  and  g  are  substituted  from  (27)  and  (42),  we  obtain 
the  decomposition  of  the  3x3  block  of  the  PLANS  state  vector  driving  noise  cov«u-iance  matrix,  as 
follows: 
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2At^ 


tn 


3T6 


cotd 


-At/T6  At 
2T6SsinP  1  -  e  (1 


p6 


^2'j<^'S^BLnffcoB0p6 


3T6" 


sAt^sin0 


(l-  e-At/T6,i.|i,] 


(50) 

(51) 


(52) 


L  =  d  -  e‘cot6 

=  0  (53) 

h  =  0  (54) 


k  =  p6(l  -  e-2^T/T6)  . 


2T6S8in0(l-  )p6 


*3  On  _ 

Bin^t/pS 


=  p6(l-e  2At/T6j 


e-At/T6(i.||.)  j%6 


(55) 


When  these  values  are  substituted  into  equation  (42)  we  see  that  the  exact  decomposition  of 
3x3  block  of  Qj(  has  the  form: 


Ok 


where: 


10  i 
0  1  cot6 
0  0  1 


k  0  0  ■ 
0  0  0 
0  0  tn 


10  0 
0  10 

i  cotB  1 


(56) 
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m  = 


sin^ 

3T6 


3X6“ 


sAt^sinfi 


1-  e-'it/Ted.ll) 


(57) 


p6(l  -  e-2At/T6)  _  gf^ 


p6 


Now  unfortunately  this  solution  does  not  satisiy  the  requirement  that  the  diagonal  elements  (k, 
1  and  m)  be  non-negative.  In  particular  it  can  be  seen  that  k  can  be  negative  by  substituting  the  model 
values  for  p6,  T6  and  At  into  equation  (57).  This  requirement  is  necessary  in  order  to  use  the  Modified 
Weighted  Gram-Schmidt  algorithm  (described  in  reference  [3]),  wh'ch  is  used  by  PLANS  to  propagate 
the  covariance  matrix. 

However,  it  is  quite  common  to  use  a  much  rougher  approximation  for  the  discrete  matrix 
than  is  used  here.  In  fact  it  is  common  to  use  QAt  in  place  of  the  integral  of  equation  (6).  This  yields  a 
which  is  diagonal,  and  hence  has  a  trivial  UDU^  decomposition.  What  has  been  done  for  PLANS 
however,  is  to  fmd  a  decomposition  which  represents  most  elements  of  Qj^  exactly  (including  the 
diagonal  terms)  and  approximates  the  others,  as  described  in  the  next  chapter.  Although  this  is  not 
entirely  exact,  it  is  much  better  than  the  usual  approximation. 
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5.  A  GOOD  APPROXIMATION  FOR  THE  3X3  DECOMPOSITION 


First  note  that  Qj^  is  not  positive  definite.  (The  rank  of  the  continuous  5x5  Q  matrix,  as  shown 
in  the  bottom  right  corner  of  equation  (3),  is  obviously  only  two.)  Therefore  the  Cholesky 
decomposition  of  Qj^  does  not  necessarily  exist  (see  for  example  reference  [4]),  as  we  have  indeed 
discovered.  Of  course  Q  is  positive  semi-definite  (since  it  is  a  covariance  matrix),  which  is  the  more 
basic  requirement  for  the  Kalman  filter  equations.  In  order  to  use  the  more  numerically  stable 
algorithms  however,  a  decomposible  approximation  to  must  be  found. 

(After  determining  that  the  exact  UDU^  decomposition  had  a  negative  diagonal  clement, 
another  decomposition  was  attempted:  the  LDL^,  which  'ises  lower  triangular  rather  than  upper 
triangular  matrices.  This  also  (perhaps  predictably?)  produced  a  negative  diagonal  element.) 

The  following  approximation  was  found  by  inspection: 


'10  a 

O 

o 

o 

10  0 

0  1  bcos0 

0  0  0 

0  1  0 

0  0  bsin0 

V  ^ 

O 

o 

o 

a  bcoafl  hsinO 

'00  ac 

O 

o 

= 

0  0  bccos^ 

0  1  0 

0  0  basing 

J 

a  bcos0  bsin0 

k  J 

a^c  abc*cos6  abc*sin0 

abc'co30  h^c'cos^d  b^c'cos^sin^ 
abc'sin^  b^c'cos^sin^  b^c'sin^^ 


(58) 


By  comparing  this  to  equation  (27),  we  .see  that  this  already  has  the  correct  6  dependence.  In 
fact  we  would  have  an  exact  solution  if  we  could  find  an  a,  b  and  c  to  satisfy  the  following: 


a^c  =  p6  ( ) 

,  2At^  , 

b^c  = 


(59) 


(60) 
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abc 


2p6  ST6  1  -  e-^t/T6(l  _ 


(61) 


This  does  not  generally  (i.e.  for  arbitrary  values  of  p6  T6  and  A  t)  have  an  exact  solution,  as  can 
be  seen  by  comparing  (59)x(60)  and  (61)^,  which  should  both  be  equal  to  (abc)^.  However,  by  solving 
(59)  and  (60)  exactly  and  approximating  (61)  we  have; 


(62) 


This  gives  an  exact  solution  on  the  diagonal  and  latitude/longitude  cross  terms  (the  (5,4)  and 
(4,5)  components  of  this  5x5  block  of  Qj^).  The  ter  o  wmch  arc  approximated  (the  (3,4)  and  (3,5) 
components)  have  the  correct  sign  and  tli"  correct  6  dependence.  Further  analysis  indicates  that  the 
approximated  terms  are  smaller  than  the  true  terms,  provided  only  that  the  propagation  interval  is 
sufficiently  short: 

At  <  T6  =  200  minutes  (63) 


which  will  certainly  be  the  case  in  PLANS.  This  can  be  seen  by  substituting  (62)  into  (58)  and 
comparing  to  (27).  Thus  this  approximation  for  Qj^  is  certainly  better  than  the  simpler  QAt 
approximation. 
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APPENDIX  A:  BLOCK  DIAGONAL  FORM  PRESERVATION 
UNDER  DECOMPOSITION 


In  this  section  we  shall  prove  that  a  block  diagonal  matrix  can  be  decomposed 
(without  loss  of  generality)  by  decomposing  its  diagonal  blocks.  This  will  be  very  useful  for 
implementation  of  Kalman  filters,  since  state  models  are  often  of  block  diagonal  form,  with  a 
separate  block  for  each  independent  sensor  or  subsystem.  Deriving  explicit  decompositions 
for  the  corresponding  driving  noise  covariance  matrices  Oj^  can  then  be  greatly  simplified. 


We  will  first  prove  the  result  for  a  matrix  with  two  blocks  on  its  diagonal.  The  extension  to  the 
general  case  is  a  straightforward  application  of  mathematical  induction. 

Consider  the  Cholesky  UDU^  decomposition  of  a  positive  definite  square  matrix  M,  which  has 
two  diagonal  blocks: 


M 


Ml  0 

0  M2 


(Al) 


Where  A  and  C  are  upper  triangular  matrices  with  one’s  on  the  diagonal,  and  D  and  E  are  diagonal 
matrices  (with  non-zero  elements  on  the  diagonal,  since  M  is  positive  definite).  Multiplying  (A2)  out 
we  obtain: 


M 


AD  BE 

'A  O' 

0  CE 

T  T 

B  C 

,  T  T  T 

f  (ADA  +BEB  )  BEC 


CEB 


CEC 


(A3) 
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Now  for  (A2)  to  be  a  decomposition  of  (Al),  the  off-diagonal  blocks  of  (A3)  must  be  zero.  Thus: 


T  ' 

CEB  =  0  (A4) 

T 

BEC  =  0  (A5) 


Since  C  is  upper  triangular  with  I’s  on  the  diagonal,  and  E  is  a  diagonal  matrix,  then  if  we  define: 


F  =  CE 


(A6) 


we  can  easily  see  that  F  is  upper  triangular  with  the  (non-zero)  elements  of  E  on  its  diagonal.  Then 
(A4)  becomes: 


T 


FB 


0 


el  X  ...  X  X 


0  e2 

0  0 
0  0 


X  X 

e(n-l)  X 


T 

B  = 


0  en  J 


0 


(A7) 


(A8) 


Close  examination  of  (A8)  gives  us  the  desired  result:  Starting  with  the  last  row  of  (A8),  we  can 
see  that  the  bottom  row  of  B^  must  be  zero  (since  e^  0).  Given  that  the  bottom  row  of  B^  is  zero, 
then  examination  of  the  second  last  row  of  (A8)  shows  that  the  second  last  row  of  B^  also  must  be  zero 
(since  ej,.i  ^  0).  This  can  be  continued  up  the  rows  to  show  that  all  rows  of  B^  are  zero.  From  (A2) 
we  can  then  see  that  the  Cholesky  decomposition  of  M  is  block  diagonal,  vvith  the  same  block  form  as 
M. 


Now  this  can  easily  be  generalized  to  a  matrix  N  with  more  than  two  blocks  by  separating  one 
block  at  a  time  as  follows.  Let  Ml  in  (Al)  be  the  top  block  of  N,  so  that  M2  contains  all  the  remaining 
blocks.  The  theorem  as  it  stands  proves  that  M2  can  be  decomposed  separately  from  Ml.  Now  simply 
apply  the  theorem  again  to  M2  to  see  that  its  top  diagonal  block  (the  second  block  of  N)  can  be 
decomposed  separately  from  the  rest  (the  third  and  remaining  blocks  of  N)  This  can  clearly  be 
repeated  until  all  the  blocks  have  been  separated. 
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APPENDIX  B:  POSITION  ERROR  COVARIANCE  PROPAGATION 

IN  PLANS 


The  time  dependence  of  the  state  vector  error  covariance  matrix  P,  in  the  absence  of 
measurement  updates,  is  described  by  the  covariance  propagation  equation  (see  for  example  reference 

[5]): 


Pt+At  =  ^(t+At)Pt«t''P(t+At)  +  Qk  (Bl) 


Since  the  latitude  and  longitude  error  estimates  are  the  last  two  elements  of  the  state  vector, 
the  position  error  covariance  is  described  by  the  last  two  diagonal  elements  of  the  covariance  matrix  P. 
Thus  we  will  examine  the  propagation  of  these  last  two  elements  of  P.  From  (Bl)  we  can  see  that  this 
involves  only  the  bottom  two  rows  of  4>  and  their  transpose  (the  last  two  columns  of  4>^).  From 
equation  (5)  we  can  see  that  the  first  four  columns  of  the  last  two  rows  of  4>  are  zero,  and  can  therefore 
be  ignored. 

Equation  (27)  can  be  used  to  obtain  the  relevant  eiemenls  of  Qj^.  Here  the  p^  refers  to  the 
steady  state  covariance  of  the  sixth  state  (the  odometer  scale  factor  error),  as  explained  by  equation 
(11).  This  is  a  constant  which  comes  from  the  error  model,  and  has  a  value  of  about  0.001 
(dimensionless). 

If  we  assign  the  Markov  process  covariances,  P(5,5)  and  P(6,6),  to  their  steady  state  values, 
then  we  would  have  a  gyro  heading  error  P(5,5)  of  about  (0.1  radian)^  and  an  odometer  scale  factor 
error  P(6,6)  of  about  (1%)^.  This  is  what  would  be  expected  in  the  absence  of  measurements,  and  its 
reasonableness  has  been  verified  by  simulation.  The  relevant  portion  of  (Bl)  can  then  be  written  as 
follows,  using  (5)  for  the  form  of  4*  and  (30)  for  the  form  of  Qj^ : 


P  t+At  - 


a  b  1  0 
.  b  -a  0  1 


'0.01  0  d  e  ' 

'  a  b  ' 

0  0.0001  f  g 

b  -a 

d  f  h  i 

1  0 

.  e  g  i  j  , 

.01, 

kooe^ 

ksindoried 


ksinAx)ed 

ksin^ 


(B2) 


where  the  position  error  covariance  before  propagation  is: 
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h 


P(7,7)t 


(B3) 


=  P(8,8)t 


(B4) 


The  elements  a  and  b  of  the  propagation  matrix^  are  shown  in  (5)  to  be: 


a 

=  -At  S  sin0 

(B5) 

b 

=  At  S  coS 

(B6) 

Assuming  a  propagation  interval  of  At  =  1  second,  the  relevant  elements  of  the  driving  noise 
covariance  Qj^  can  be  found  from  equation  (30),  which  implies  that : 

k  =  0.0002  P6  S2  (B7) 


Multiplying  (B2)  out  we  obtain: 


Pt+At  = 

(0.01a  +  d) 
(O.Olb  +  e) 


(0.0001b  +  f) 
(-O.OOOla  +  g) 


(ad  +  bf  +  h) 
(bd  -af  +  i) 


(ae  +  bg  +  i) 
(be  -  ag  +  j) 


r  a  b  'I 


b  -a 
1  0 


+  (B8) 


I  0  1  J 


( 0 . Ola^+ad+O . OOOlb^+bf +ad+bf +h) 


(O.Olb^+be+O.OOOla^-ag+be-ag+j ) 


Qk 


Therefore,  the  position  error  covariance  terms  are: 


P(7,7),+At  = 


(O.Ola^  +  O.OOOlb^  +  2(ad  +  bf)  +  h)  +  kcoe^B  (B9) 

P(7,7)j  +  0.01a2  +  O.OOOlb^  +  kcos^^  +  2  (ad  +  bf) 

P(7,7)t  +  S2(0.01sin^  +  OOlOlcos^  +  OOnDOBcos^)  +  2S(  -suSd  +  co^f)  (BIO) 
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P(8,8)t+^t=  (O.Olb^  +  O.OOOla^  +  2(be  -  ag)  +  j))  +  ksin^^ 


(Bll) 


=  P(8,8),  +  O.Olb^  +  O.OOOla^  +  ksin^d  +  2  (be  -  ag) 

S  P(8.8)t  +  S2(0.01cos^  +  OflOOlsin^e  +  OyOOOOOQEsin^ )  +  2S(  co^e  +  suSe.)  (B12) 


Thus  the  position  covariance  can  decrease  in  the  absence  of  measurements,  if  the  underlined 
terms  in  equations  (BIO)  and  (B12)  are  large  enough  in  the  negative  sense.  This  will  happen  for  certain 
values  of  heading  6  and  speed  S,  provided  the  d,  e,  f  and  g  terms  are  not  too  small.  Simulations  have 
shown  that  these  terms  can  be  large  enough  to  cause  P(7,7)  and  P(8,8)  to  decrease,  particularly  in  the 
absence  of  position  measurements.  The  physical  interpretation  is  that  while  the  vehicle  is  heading  back 
towards  its  point  of  origin  the  heading  and  speed  errors  start  to  cancel  the  "outboimd"  errors.  In  the 
absence  of  position  update  measurements  (from  GPS  or  Transit)  the  outbound  position  errors  will  of 
course  be  caused  entirely  by  the  heading  and  speed  errors,  and  will  therefore  be  statistically  correlated 
to  them  through  the  cross  covariances  d,  e,  f  and  g. 
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